/*
 *	By Yinpeng Li, mousquetaires@unc.edu
 */

#include <CounterStatistics.h>
#include <FiberODF_Common.h>

double gaussian(const double coefficient, const double disFromCenter, const double sigma){
	return coefficient * exp(- pow(disFromCenter, 2) / 2 / pow(sigma, 2)) ;
}

SingleGaussianFunctor::SingleGaussianFunctor(Counter &c):
		vnl_least_squares_function(4,	//Four parameters: phi, theta, coefficient, sigma
					   4,	//Required by the vnl levenberg marquardt to be at least of the same dimension with the parameters
					   no_gradient),
		counter(c){
}

void SingleGaussianFunctor::f(vnl_vector<double> const& x, vnl_vector<double>& fx){
	VectorType center = counter.GetCartesianCoordinates(x[0], x[1]) ;
	geometry::Vector vecToCenter(center[0], center[1], center[2]) ;
	
	double res = 0 ;
	
	for (int i = 0; i < counter.GetSize(); i++){
		VectorType vertex = counter.GetBinCartesianCoordinates(i) ;
		geometry::Vector vecToVertex(vertex[0], vertex[1], vertex[2]) ;
		
		if (vecToCenter * vecToVertex <= 0)	//Exclude all vertices not on the same half of the sphere
			continue ;
		
		const double arcLength = acos(vecToCenter * vecToVertex) ;
		
		res += pow(counter.GetBinValue(i) - gaussian(x[2], arcLength, x[3]), 2) ;
	}
	
	fx[0] = res ;
}
